#pragma once

#ifdef min
#undef min
#endif

#ifdef max
#undef max
#endif

#include "NxPhysics.h"
#include "NxCooking.h"
#include "NXU_helper.h"
#include "NxDebugRenderable.h" 
#include "NXU_Streaming.h"


namespace _MODULE_
{
	namespace _PHYSICS_
	{

		extern NxVec3 NxDIRECTION_FRONT,NxDIRECTION_RIGHT,NxDIRECTION_UP;
#define PhysXDegToRad(deg) (deg*NxPi/180)
#define PhysXRadToDeg(rad) (rad*180/NxPi)

		__forceinline void PhysX_FromEulerAnglesYZX(/*out*/NxMat33& out,float fYAngle, float fZAngle,float fXAngle)
		{
			NxMat33 kXRot, kYRot, kZRot;
			kXRot.rotX(fXAngle);
			kYRot.rotY(fYAngle);
			kZRot.rotZ(fZAngle);
			out = kYRot*(kZRot*kXRot);
			
		}

		__forceinline void PhysX_FromAxisAngle (/*out*/NxMat33& out,float fAngle,float fx, float fy,float fz)
		{
			double fCos = cos(-fAngle);
			double fSin = sin(-fAngle);
			double  fOneMinusCos = 1.0-fCos;
			double  fX2 = fx*fx;
			double  fY2 = fy*fy;
			double  fZ2 = fz*fz;
			double  fXYM = fx*fy*fOneMinusCos;
			double  fXZM = fx*fz*fOneMinusCos;
			double  fYZM = fy*fz*fOneMinusCos;
			double  fXSin = fx*fSin;
			double  fYSin = fy*fSin;
			double  fZSin = fz*fSin;
		    
			out.setRow(0,NxVec3(fX2*fOneMinusCos+fCos,fXYM-fZSin,fXZM+fYSin));
			//m_afEntry[0] = fX2*fOneMinusCos+fCos;
			//m_afEntry[1] = fXYM-fZSin;
			//m_afEntry[2] = fXZM+fYSin;
			out.setRow(1,NxVec3(fXYM+fZSin,fY2*fOneMinusCos+fCos,fYZM-fXSin));
			//m_afEntry[3] = fXYM+fZSin;
			//m_afEntry[4] = fY2*fOneMinusCos+fCos;
			//m_afEntry[5] = fYZM-fXSin;
			out.setRow(2,NxVec3(fXZM-fYSin,fYZM+fXSin,fZ2*fOneMinusCos+fCos));
			//m_afEntry[6] = fXZM-fYSin;
			//m_afEntry[7] = fYZM+fXSin;
			//m_afEntry[8] = fZ2*fOneMinusCos+fCos;
		}
		__forceinline void PhysX_ToAxisAngle(/*in*/NxMat33& in,double& rfAngle,NxVec3& rkAxis)
		{
			 // Let (x,y,z) be the unit-length axis and let A be an angle of rotation.
			// The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where
			// I is the identity and
			//
			//       +-        -+
			//   P = |  0 -z +y |
			//       | +z  0 -x |
			//       | -y +x  0 |
			//       +-        -+
			//
			// If A > 0, R represents a counterclockwise rotation about the axis in
			// the sense of looking from the tip of the axis vector towards the
			// origin.  Some algebra will show that
			//
			//   cos(A) = (trace(R)-1)/2  and  R - R^t = 2*sin(A)*P
			//
			// In the event that A = pi, R-R^t = 0 which prevents us from extracting
			// the axis through P.  Instead note that R = I+2*P^2 when A = pi, so
			// P^2 = (R-I)/2.  The diagonal entries of P^2 are x^2-1, y^2-1, and
			// z^2-1.  We can solve these for axis (x,y,z).  Because the angle is pi,
			// it does not matter which sign you choose on the square roots.

			NxVec3 row0,row1,row2;
			in.getRow(0,row0);
			in.getRow(1,row1);
			in.getRow(2,row2);

			double fTrace = row0[0] + row1[1] + row2[2];
			double fCos = 0.5*(fTrace-1.0);
			rfAngle = acos(fCos);  // in [0,PI]

			if (rfAngle > 0.0)
			{
				if (rfAngle < NxPiF32)
				{
					rkAxis.x = row2[1]-row1[2];
					rkAxis.y = row0[2]-row2[0];
					rkAxis.z = row1[0]-row0[1];
					rkAxis.normalize();
				}
				else
				{
					// angle is PI
					double fHalfInverse;
					if (row0[0] >= row1[1])
					{
						// r00 >= r11
						if (row0[0] >= row2[2])
						{
							// r00 is maximum diagonal term
							rkAxis.x = 0.5*sqrt(row0[0] -
								row1[1] - row2[2] + 1.0);
							fHalfInverse = 0.5/rkAxis.x;
							rkAxis.y = fHalfInverse*row0[1];
							rkAxis.z = fHalfInverse*row0[2];
						}
						else
						{
							// r22 is maximum diagonal term
							rkAxis.z = 0.5*sqrt(row2[2] -
								row0[0] - row1[1] + 1.0);
							fHalfInverse = 0.5/rkAxis.z;
							rkAxis.x = fHalfInverse*row0[2];
							rkAxis.y = fHalfInverse*row1[2];
						}
					}
					else
					{
						// r11 > r00
						if (row1[1] >= row2[2])
						{
							// r11 is maximum diagonal term
							rkAxis.y = 0.5*sqrt(row1[1] -
								row0[0] - row2[2] + 1.0);
							fHalfInverse  = 0.5/rkAxis.y;
							rkAxis.x = fHalfInverse*row0[1];
							rkAxis.z = fHalfInverse*row1[2];
						}
						else
						{
							// r22 is maximum diagonal term
							rkAxis.z = 0.5*sqrt(row2[2] -
								row0[0] - row1[1] + 1.0);
							fHalfInverse = 0.5/rkAxis.z;
							rkAxis.x = fHalfInverse*row0[2];
							rkAxis.y = fHalfInverse*row1[2];
						}
					}
				}
			}
			else
			{
				// The angle is 0 and the matrix is the identity.  Any axis will
				// work, so just use the x-axis.
				rkAxis.x = (float)1.0;
				rkAxis.y = (float)0.0;
				rkAxis.z = (float)0.0;
			}
		}
		__forceinline void PhysX_FromEulerAnglesYZXInverse(/*out*/NxMat33& out,float fYAngle, float fZAngle,float fXAngle)
		{
			
			NxMat33 kXRot, kYRot, kZRot;
			kXRot.rotX(-fXAngle);
			kYRot.rotY(-fYAngle);
			kZRot.rotZ(-fZAngle);
			out = kXRot*(kZRot*kYRot);
		}

		__forceinline float PhysX_NiASin (float fValue)
		{
			if ( -1.0f < fValue )
			{
				if ( fValue < 1.0f )
					return float(asin(fValue));
				else
					return NxHalfPiF32;
			}
			else
			{
				return -NxHalfPiF32;
			}
		}

		__forceinline float PhysX_NiFastATan2(float fY, float fX)
		{
			// Poly approximation valid for |z| <= 1.  To compute ATAN(z)
			// for z > 1, use ATAN(z) = PI/2 - ATAN(1/z).  For z < -1, use
			// ATAN(z) = -PI/2 - ATAN(1/z).

			if (fX == 0.0f && fY == 0.0f)
				return 0.0f;

			float fOffset = 0.0f;
			float fZ;
			if (abs(fY) > abs(fX))
			{
				//  |fY/fX| > 1 so use 1/z identities.
				fZ = fX / fY;
				if (fZ > 0.0f)
				{
					fOffset = NxHalfPiF32;
				}
				else if (fZ < 0.0f)
				{
					fOffset = -NxHalfPiF32;
				}
				else // fZ == 0.0f
				{
					// special case for 90deg and -90deg
					return (fY > 0.0f) ? NxHalfPiF32 : -NxHalfPiF32;
				}
			}
			else
			{
				fZ = fY / fX;

				if (fZ == 0.0f)
				{
					// special case for 0deg and 180deg
					return (fX > 0.0f) ? 0.0f : NxPiF32 ;
				}
			}

			float fZ2 = fZ * fZ;

			// Polynomial approximation of degree 9, P(z).
			// |ATAN(z)-P(z)| <= 1e-05

			float fResult;
			fResult = 0.0208351f;
			fResult *= fZ2;
			fResult -= 0.0851330f;
			fResult *= fZ2;
			fResult += 0.1801410f;
			fResult *= fZ2;
			fResult -= 0.3302995f;
			fResult *= fZ2;
			fResult += 0.9998660f;
			fResult *= fZ;

			if (fOffset)
				fResult = fOffset - fResult;

			// find proper solution to two arg arctan
			if (fY < 0.0f && fX < 0.0f)  // quadrant IV
				fResult -= NxPiF32 ;
			if (fY > 0.0f && fX < 0.0f)  // quadrant II
				fResult += NxPiF32 ;

			// assert(NiAbs(fResult - NiATan2(fY, fX)) <= 1e-05);

			return fResult;
		}

		__forceinline bool PhysX_ToEulerAnglesYZX(/*in*/NxMat33& in,float& rfYAngle, float& rfZAngle,float& rfXAngle)
		{
			NxVec3 row0,row1,row2;
			in.getRow(0,row0);
			in.getRow(1,row1);
			in.getRow(2,row2);

			
			if ( row1[0] < 1.0 )
			{
				if (  row1[0]  > -1.0 )
				{
					rfYAngle = PhysX_NiFastATan2(-row2[0],row0[0]);
					rfZAngle = PhysX_NiASin(row1[0]);
					rfXAngle = PhysX_NiFastATan2(-row1[2],row1[1]);
					return true;
				}
				else
				{
					// WARNING.  Not a unique solution.
					rfXAngle = 0.0f;  
					rfZAngle = -NxHalfPiF32;
					rfYAngle = -PhysX_NiFastATan2(row2[1],row2[2]);
					return false;
				}
			}
			else
			{
				// WARNING.  Not a unique solution.
				rfXAngle = 0.0f;  // any angle works
				rfZAngle = NxHalfPiF32;
				rfYAngle = PhysX_NiFastATan2(row2[1],row2[2]);
				return false;
			}
		}

		__forceinline void PhysX_CopyShapes(NxActor* ptrActorTo,NxActor* ptrActorFrom)
		{
			//copy shape
			NxShape *const* arrayShape=ptrActorFrom->getShapes();
			NxMat34 childGlobalPose,parentInverseGlobalPose;
			ptrActorTo->getGlobalPose().getInverseRT(parentInverseGlobalPose);

			for(int i=0;i<ptrActorFrom->getNbShapes();i++)
			{
				NxShape * ptrShape=arrayShape[i];

				if(ptrShape->isBox())
				{
					NxBoxShapeDesc shape;
					ptrShape->isBox()->saveToDesc(shape);

					childGlobalPose=ptrShape->getGlobalPose();
					shape.localPose=parentInverseGlobalPose*childGlobalPose;

					shape.shapeFlags|=NX_SF_VISUALIZATION;

					ptrActorTo->createShape(shape);
				}
				else if(ptrShape->isCapsule())
				{
					NxCapsuleShapeDesc shape;
					ptrShape->isCapsule()->saveToDesc(shape);

					childGlobalPose=ptrShape->getGlobalPose();
					shape.localPose=parentInverseGlobalPose*childGlobalPose;

					shape.shapeFlags|=NX_SF_VISUALIZATION;


					ptrActorTo->createShape(shape);
				}
				else if(ptrShape->isConvexMesh())
				{
					NxConvexShapeDesc shape;
					ptrShape->isConvexMesh()->saveToDesc(shape);

					childGlobalPose=ptrShape->getGlobalPose();
					shape.localPose=parentInverseGlobalPose*childGlobalPose;

					shape.shapeFlags|=NX_SF_VISUALIZATION;


					ptrActorTo->createShape(shape);
				}
				else if(ptrShape->isHeightField())
				{
					NxHeightFieldShapeDesc shape;
					ptrShape->isHeightField()->saveToDesc(shape);

					childGlobalPose=ptrShape->getGlobalPose();
					shape.localPose=parentInverseGlobalPose*childGlobalPose;

					shape.shapeFlags|=NX_SF_VISUALIZATION;


					ptrActorTo->createShape(shape);
				}
				else if(ptrShape->isPlane())
				{
					NxPlaneShapeDesc shape;
					ptrShape->isPlane()->saveToDesc(shape);

					childGlobalPose=ptrShape->getGlobalPose();
					shape.localPose=parentInverseGlobalPose*childGlobalPose;

					shape.shapeFlags|=NX_SF_VISUALIZATION;


					ptrActorTo->createShape(shape);
				}
				else if(ptrShape->isTriangleMesh())
				{
					NxTriangleMeshShapeDesc shape;
					ptrShape->isTriangleMesh()->saveToDesc(shape);

					childGlobalPose=ptrShape->getGlobalPose();
					shape.localPose=parentInverseGlobalPose*childGlobalPose;

					shape.shapeFlags|=NX_SF_VISUALIZATION;


					ptrActorTo->createShape(shape);
				}
				else if(ptrShape->isWheel())
				{
					NxWheelShapeDesc shape;
					ptrShape->isWheel()->saveToDesc(shape);

					childGlobalPose=ptrShape->getGlobalPose();
					shape.localPose=parentInverseGlobalPose*childGlobalPose;

					shape.shapeFlags|=NX_SF_VISUALIZATION;


					ptrActorTo->createShape(shape);
				}
			}

			ptrActorTo->updateMassFromShapes(1,0);
		}

	}
}
